package edu.wpi.first.wpilibj.templates;



import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;


public class RobotMain extends SimpleRobot 
{
    
    //pwm ids for drive motor control
    private int fL = 1; //pwm id for front left motor
    private int fR = 3; //pwm id for front right motor
    private int rL = 2; //pwm id for rear left motor
    private int rR = 4; //pwm id for rear right motor
    private int shooterPort = 5; //pwm id for the shooter
    private int launcher = 1; //solenoid id for the launcher 
    private int compressorPort = 2; // Digital IO port for the Pressure Switch
    private int spikePort = 1; //Relay port for the Spike
    
    //Control declaration
    private Joystick leftJoystick;
    private Joystick rightJoystick;
    
    //Camera declarations
    private AxisCamera axisCamera; 
    private Camera camera;
    
    private RobotDrive drive; 
    private Kinect autoDrive;
    
    private Shooter shooter;
    private double shooterSpeed;
    private Compressor airCompressor;
    private boolean shooterOnOff = true;
    
    private double previousButtonPress;
    private double delayForButtonPress;
    private double previousLauncherButtonPress;
    private double delayForLauncherButtonPress;
    private double previousAutoShoot;
    private double delayForAutoShoot;
    
    public void robotInit()
    {
        drive = new RobotDrive(fL, fR, rL, rR);
        leftJoystick = new Joystick(2);
        rightJoystick = new Joystick(1);
        
        shooterSpeed = 0;
        shooter = new Shooter(shooterPort, shooterSpeed, launcher);
        autoDrive = new Kinect(1,2);
        
        airCompressor = new Compressor(compressorPort, spikePort);
      
        delayForButtonPress = 2;
        previousButtonPress = 0;
        delayForLauncherButtonPress = .25;
        previousLauncherButtonPress = 0;
        
        axisCamera = AxisCamera.getInstance("10.17.45.11");
        axisCamera.writeResolution(AxisCamera.ResolutionT.k160x120);
        axisCamera.writeMaxFPS(15);
        axisCamera.writeCompression(20);
        axisCamera.writeColorLevel(50);
        axisCamera.writeBrightness(40);
        camera = new Camera(axisCamera);
    }
    
    public void autonomous() 
    {
        System.err.println("Entering autonomous:");
        airCompressor.start();
        System.err.println("[AUTO]Compressor ON");
        int amountToShoot = 0;
        shooter.setTargetSpeed(-1);
         while (isAutonomous() && isEnabled()) 
         {
            //Camera.imageGrab(camera, collection);
            Timer.delay(4);
            while(amountToShoot <= 3)
            {
                previousAutoShoot = Timer.getFPGATimestamp();
                if(amountToShoot == 0)
                {
                    shooter.setTargetSpeed(-0.95);
                }
                else
                {
                    shooter.setTargetSpeed(-1);
                }
                System.err.println("[AUTO]Frisbee number " + amountToShoot + " Shooter Speed " + shooter.getSpeed());
                System.err.println("[AUTO]Firing Shooter");
                shooter.fire(); 
                Timer.delay(.25);
                shooter.reset();
                System.err.println("[AUTO]Reseting Shooter");
                amountToShoot++;
                Timer.delay(2);
            }
            shooter.setTargetSpeed(0);
         }
     }
    
    
    public void operatorControl() 
    {
        System.err.println("[TeleOP]Entering teleop:");
        airCompressor.start();
        System.err.println("[TeleOP]Compressor ON");
        shooter.setTargetSpeed(0);
        while(isOperatorControl() && isEnabled())
        {
            drive.tankDrive (-rightJoystick.getY(), -leftJoystick.getY());
            
              /*Flywheel speed tweaking code*/
//            if(leftJoystick.getRawButton(10)) //decrease the speed
//            {
//                if(shooter.getSpeed() < 0 && shooter.getSpeed() != 0 && (Timer.getFPGATimestamp() - previousButtonPress >= delayForButtonPress))
//                {
//                    previousButtonPress = Timer.getFPGATimestamp();
//                    shooterSpeed = shooterSpeed + 0.05;
//                    shooter.setTargetSpeed(shooterSpeed);
//                    System.err.println("Decreasing Shooter Speed: " + shooterSpeed);
//                }
//            }
//            if(leftJoystick.getRawButton(11)) //increse the speed
//            {
//                if(shooter.getSpeed() > -1 && (Timer.getFPGATimestamp() - previousButtonPress >= delayForButtonPress))
//                {
//                    previousButtonPress = Timer.getFPGATimestamp();
//                    shooterSpeed = shooterSpeed - 0.05;
//                    shooter.setTargetSpeed(shooterSpeed);
//                    System.err.println("Increasing Shooter Speed: " + shooterSpeed);
//                }
//            }
            
            if(rightJoystick.getRawButton(1)) // if the fire button is pressed shoot
            {
                if(shooter.getSpeed() != 0)
                {
                    previousLauncherButtonPress = Timer.getFPGATimestamp();
                    System.err.println("[TeleOP]Firing Shooter");
                    shooter.fire();
                } 
            }
            else if(Timer.getFPGATimestamp() - previousLauncherButtonPress >= delayForLauncherButtonPress)
            {
                shooter.reset();
            }
           
            if(leftJoystick.getRawButton(1)) //turn the shooter off and on
            {
                if(Timer.getFPGATimestamp() - previousButtonPress >= delayForButtonPress)
                {
                    previousButtonPress = Timer.getFPGATimestamp();
                    if(shooterOnOff)
                    {
                        System.err.println("[TeleOP]Shooter On");
                        shooter.setTargetSpeed(-1);
                        shooterOnOff = false;
                    }
                    else
                    {
                        System.err.println("[TeleOP]Shooter Off");
                        shooter.setTargetSpeed(0);
                        shooterOnOff = true;
                    }
                }
            }
            
        }
        
    }
   
   public void test()
   {
       System.err.println("[TEST]Entering Test:");
       while(isTest() && isEnabled())
       {   
           //if(/*autoDrive.getInstance().getSkeleton().GetTrackState() == Skeleton.tTrackState.kTracked && */autoDrive.getDeadZone()) 
            //{
               drive.tankDrive(-autoDrive.getLeftY(), -autoDrive.getRightY());
            
            //}
            //else
            //{
            //    drive.tankDrive(0,0);
            //    Timer.delay(.01);
            //}
       }
   }
}